import math
from queue import PriorityQueue

from matplotlib import pyplot as plt


class AStarPlanner:
    def __init__(self, map, wh=1):
        self.obmap = map
        self.minx = 0
        self.maxx = self.obmap.shape[0]
        self.miny = 0
        self.maxy = self.obmap.shape[1]
        self.motion = self.get_motion_model()
        self.wh = wh

    class Node:
        def __init__(self, x, y, cost):
            self.x = x
            self.y = y
            self.cost = cost

        def __str__(self):
            return "[" + str(self.x) + " ," + str(self.y) + "] ," + str(
                self.cost)

        def __eq__(self, other):
            return self.x == other.x and self.y == other.y

        def __hash__(self):
            return hash((self.x, self.y))

        def __lt__(self, other):
            return self.cost < other.cost

    def neighbors(self, current):
        neighbors = []
        for i in range(len(self.motion)):
            node = self.Node(current.x + self.motion[i][0],
                             current.y + self.motion[i][1],
                             current.cost + self.motion[i][2])
            if not self.verify_node(node):
                continue
            neighbors.append(node)
        return neighbors

    def heuristic(self, a, b):
        return math.hypot(a.x - b.x, a.y - b.y)
        # return abs(a.x - b.x) + abs(a.y - b.y)

    def is_line_of_sight(self, n1, n2):
        # 计算n1和n2之间的距离
        dx = n2[0] - n1[0]
        dy = n2[1] - n1[1]
        d = math.hypot(dx, dy)

        # 计算n1和n2之间的单位向量
        ux = dx / d
        uy = dy / d

        # 计算n1和n2之间的距离
        x = n1[0]
        y = n1[1]
        dist = 0

        while dist < d:
            x += ux
            y += uy

            if int(x) < self.minx:
                return False
            elif int(y) < self.miny:
                return False
            elif int(x) >= self.maxx:
                return False
            elif int(y) >= self.maxy:
                return False

            if self.obmap[int(x)][int(y)]:
                return False
            dist += 1

        return True

    def greedy_remove_redundant_nodes(self, path):
        optimized_path = [path[0]]
        current_node = path[0]
        i = 1

        while i < len(path) - 1:
            next_node_in_line_of_sight = i
            save_i = i
            while next_node_in_line_of_sight < len(path):
                if self.is_line_of_sight(current_node, path[next_node_in_line_of_sight]):
                    save_i = next_node_in_line_of_sight
                next_node_in_line_of_sight += 1

            optimized_path.append(path[save_i])
            current_node = path[save_i]
            i = save_i + 1

        optimized_path.append(path[-1])

        return optimized_path

    def planning(self, start_point, goal_point):
        start = self.Node(start_point[0], start_point[1], 0.0)

        goal = self.Node(goal_point[0], goal_point[1], 0.0)

        frontier = PriorityQueue()
        frontier.put((0, start))
        came_from = dict()
        cost_so_far = dict()
        came_from[start] = None
        cost_so_far[start] = 0

        while not frontier.empty():
            current = frontier.get()[1]

            if current == goal:
                break

            # plt.plot(current.x,
            #          current.y,
            #          "xc")  # 青色x 搜索点
            # # 按esc退出
            # plt.gcf().canvas.mpl_connect('key_release_event',
            #                              lambda event: [exit(0) if event.key == 'escape' else None]
            #                              )
            # #每10个点暂停0.01s
            # if len(came_from.keys()) % 100 == 0:
            #     plt.pause(0.01)

            for next in self.neighbors(current):
                if next not in cost_so_far or next.cost < cost_so_far[next]:
                    cost_so_far[next] = next.cost
                    priority = next.cost + self.wh * self.heuristic(goal, next)
                    frontier.put((priority, next))
                    came_from[next] = current

        path = []
        # 如果goal不在came_from中，说明没有找到路径
        if goal not in came_from:
            return path, path

        current = goal
        while current != start:
            path.append([current.x + 0.5, current.y + 0.5])
            current = came_from[current]
        path.append([start.x + 0.5, start.y + 0.5])
        path.reverse()

        new_path = self.greedy_remove_redundant_nodes(path)

        return path, new_path

    @staticmethod
    def calc_heuristic(n1, n2):
        # 曼哈顿距离
        return abs(n1.x - n2.x) + abs(n1.y - n2.y)

    def verify_node(self, node):
        if node.x < self.minx:
            return False
        elif node.y < self.miny:
            return False
        elif node.x >= self.maxx:
            return False
        elif node.y >= self.maxy:
            return False

        if self.obmap[int(node.x)][int(node.y)]:
            return False

        return True

    @staticmethod
    def get_motion_model():
        # dx, dy, cost
        motion = [
            [1, 0, 1],
            [0, 1, 1],
            [-1, 0, 1],
            [0, -1, 1],
            [1, 1, math.sqrt(2)],
            [1, -1, math.sqrt(2)],
            [-1, 1, math.sqrt(2)],
            [-1, -1, math.sqrt(2)]
        ]

        return motion
